/**
 * @file Dog.c
 * @brief
 * @author Luke (AiNuozhou@bupt.edu.cn)
 * @version 1.0
 * @date 2022-02-05
 *
 * @copyright Copyright (c) 2022  北京邮电大学
 *
 * @par 修改日志:
 * @date 2022-02-05
 * @author Luke (AiNuozhou@bupt.edu.cn)
 */

#include "Dog.h"
#include "flash.h"
#include "state.h"
#include "user_math.h"

Dog_s Dog;
Time_s Dog_time = {0, 0, 0};
PID_s RollPID = {1.2, 0.001, 0.02, 0, 0, 10, 0, 1, 1};
PID_s Pitch_PID = {1.2, 0.001, 0.02, 0, 0, 10, 0, 1, 1};

uint8_t speed_mode = 0;

float delta_h = 0;
float delta_z = -0.03; // 前后重心
float delta_x = -0.010;     // 左右重心
float delta_t = 0;
float delta_dh[4];
float delta_z_front = 0;
float delta_z_back = 0;

float GAIT_S = 0.04;    //前进距离
float GAIT_H = 0.02;    //抬腿高度
float GAIT_T = 6;       //每个步态的时间周期
float GAIT_DZ = 0.0265; // sqrt(0.220^2-0.060^2)/8;  %%要重新算，或以实际值为准

float gait_delta_x = 0;
float gait_delta_y = 0;
float gait_delta_z = 0.01;
float gait_delta_gain = 2;

float turn_angle = 0;

float barycenter_front = 0.02;
float barycenter_back = 0.02;
float barycenter_left = 0.02;
float barycenter_right = 0.02;
float barycenter_central = 0.02;

float phase_graph[4][2] = {{1, 1}, {-1, 1}, {-1, -1}, {1, -1}};

int debug_leg = 0;
float debug_target_x = 0;
float debug_target_y = 0;
float debug_target_z = 0;

int Up_Order=0;
float Last_t=0;

double error_x[2]={0};
uint8_t movecmp_flag=0;
int angle_num=0;
double all_angle;

void Dog_init()
{
    Dog.legs[0].servo[0].ID = 0;
    Dog.legs[0].servo[1].ID = 1;
    Dog.legs[0].servo[2].ID = 2;
    Dog.legs[0].foot_init_x = LEG0_INIT_POS_X;
    Dog.legs[0].foot_init_y = LEG0_INIT_POS_Y;
    Dog.legs[0].foot_init_z = LEG0_INIT_POS_Z;
    Dog.legs[0].configuration = 1;
    Dog.legs[0].No = 0;

    Dog.legs[1].servo[0].ID = 10;
    Dog.legs[1].servo[1].ID = 11;
    Dog.legs[1].servo[2].ID = 12;
    Dog.legs[1].foot_init_x = LEG1_INIT_POS_X;
    Dog.legs[1].foot_init_y = LEG1_INIT_POS_Y;
    Dog.legs[1].foot_init_z = LEG1_INIT_POS_Z;
    Dog.legs[1].configuration = 1;
    Dog.legs[1].No = 1;

    Dog.legs[2].servo[0].ID = 20;
    Dog.legs[2].servo[1].ID = 21;
    Dog.legs[2].servo[2].ID = 22;
    Dog.legs[2].foot_init_x += LEG2_INIT_POS_X;
    Dog.legs[2].foot_init_y = LEG2_INIT_POS_Y;
    Dog.legs[2].foot_init_z = LEG2_INIT_POS_Z;
    Dog.legs[2].configuration = 2;
    Dog.legs[2].No = 2;

    Dog.legs[3].servo[0].ID = 30;
    Dog.legs[3].servo[1].ID = 31;
    Dog.legs[3].servo[2].ID = 32;
    Dog.legs[3].foot_init_x = LEG3_INIT_POS_X;
    Dog.legs[3].foot_init_y = LEG3_INIT_POS_Y;
    Dog.legs[3].foot_init_z = LEG3_INIT_POS_Z;
    Dog.legs[3].configuration = 2;
    Dog.legs[3].No = 3;

    Dog.ctrl_mode = DOG_MODE_WALK;
    Dog.pose_mode = 2;
    Dog.target_pitch = 0;
    Dog.target_roll = 0;
    Dog.target_yaw = 0;
    for (int i = 0; i < 4; i++)
    {
        Dog.phase[i] = i;
    }
}

void angle_init();
void Update_error();
void Update_angle();
void Dog_Gait_Update_downstair();
void Dog_Gait_Update_upstair();
void Dog_Gait_Update_balance();
void Dog_Gait_Update_walk();
void Dog_Gait_Test_walk();
void Dog_Gait_Test(){
    for (int i = 3; i <=4; i++)
    {
        uprintf("%d ", UART3_TaskBuffer[i]);
    }
    uprintf("\r\n");
//    float t = fmod((Dog_time.s), 2);
//    Update_angle();

//    Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x ;
//    Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
//    Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z+0.04;
//    Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x;
//    Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
//    Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z-0.01;
//    Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x;
//    Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y+0.02;
//    Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z;
//    Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x;
//    Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
//    Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z-0.01;
//    Dog_Inverse_kinematics(&Dog.legs[0]);
//    Dog_Inverse_kinematics(&Dog.legs[1]);
//    Dog_Inverse_kinematics(&Dog.legs[2]);
//    Dog_Inverse_kinematics(&Dog.legs[3]);
//    Dog_servo_angle_pos();
    return;
}
void Dog_exe()
{
    switch (Dog.ctrl_mode)
    {
    case DOG_MODE_NONE:
    {
        Dog_Gait_Test();
        break;
    }
    case DOG_MODE_STOP:
    {
        Servo_Sync_Torque(60);
        Dog.ctrl_mode = DOG_MODE_NONE;
        break;
    }
    case DOG_MODE_RESET:
    {
        Servo_Sync_Torque(1);
        Dog_Gait_Reset();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        Dog.ctrl_mode = DOG_MODE_NONE;
        break;
    }
    case DOG_MODE_ADJUST:
    {
        Dog_Pose_Adjust();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // Dog.ctrl_mode = DOG_MODE_NONE;
        break;
    }
    case DOG_MODE_BALANCE:
    {
        Dog_Gait_Update_balance();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        break;
    }
    case DOG_MODE_WALK:
    {
        Dog_Gait_Test();
        Dog_Gait_Test_walk();
//        Dog_Gait_Update_downstair();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        break;
    }
    case DOG_MODE_WALK_OMNI:
    {
        Dog_Gait_Walk_Omni();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        break;
    }
    case DOG_MODE_PACE:
    {
        Dog_Gait_Pace();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        break;
    }
    case DOG_MODE_PACE_OMNI:
    {
        Dog_Gait_Pace_Omni();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        break;
    }
    case DOG_MODE_UPSTAIR:
    {
        Dog_Gait_Update_upstair();
//        Dog_Gait_shangtaijie_zuoyou();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        break;
    }
    case DOG_MODE_DEBUG:
    {
        Dog_Gait_swing();
        // Dog_Leg_Point_test(debug_leg, debug_target_x, debug_target_y, debug_target_z);
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        break;
    }
    case DOG_MODE_DOWNSTAIR:
    {
        Dog_Gait_Update_downstair();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        break;
    }
    case DOG_MODE_TEST:
    {
        Dog_Gait_Test();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        break;
    }
    default:
    {
        break;
    }
    }
}
void Dog_Gait_Reset()
{
    Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x;
    Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
    Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z;
    Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x;
    Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
    Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z;
    Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x;
    Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
    Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z;
    Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x;
    Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
    Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z;
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);
    Dog_servo_angle_pos();

    uprintf("Dog_Gait_Reset\n\r");
}

void Dog_Gait_Init()
{
    Dog.legs[0].foot_init_x = LEG0_INIT_POS_X;
    Dog.legs[0].foot_init_y = LEG0_INIT_POS_Y;
    Dog.legs[0].foot_init_z = LEG0_INIT_POS_Z;
    Dog.legs[1].foot_init_x = LEG1_INIT_POS_X;
    Dog.legs[1].foot_init_y = LEG1_INIT_POS_Y;
    Dog.legs[1].foot_init_z = LEG1_INIT_POS_Z;
    Dog.legs[2].foot_init_x = LEG2_INIT_POS_X;
    Dog.legs[2].foot_init_y = LEG2_INIT_POS_Y;
    Dog.legs[2].foot_init_z = LEG2_INIT_POS_Z;
    Dog.legs[3].foot_init_x = LEG3_INIT_POS_X;
    Dog.legs[3].foot_init_y = LEG3_INIT_POS_Y;
    Dog.legs[3].foot_init_z = LEG3_INIT_POS_Z;
}

void Dog_Gait_Reinit()
{
    Dog.legs[0].foot_init_x = Dog.legs[0].foot_target_x - Dog.legs[0].foot_init_x;
    Dog.legs[0].foot_init_y = Dog.legs[0].foot_target_y - Dog.legs[0].foot_init_y;
    Dog.legs[0].foot_init_z = Dog.legs[0].foot_target_z - Dog.legs[0].foot_init_z;
    Dog.legs[1].foot_init_x = Dog.legs[1].foot_target_x - Dog.legs[1].foot_init_x;
    Dog.legs[1].foot_init_y = Dog.legs[1].foot_target_y - Dog.legs[1].foot_init_y;
    Dog.legs[1].foot_init_z = Dog.legs[1].foot_target_z - Dog.legs[1].foot_init_z;
    Dog.legs[2].foot_init_x = Dog.legs[2].foot_target_x - Dog.legs[2].foot_init_x;
    Dog.legs[2].foot_init_y = Dog.legs[2].foot_target_y - Dog.legs[2].foot_init_y;
    Dog.legs[2].foot_init_z = Dog.legs[2].foot_target_z - Dog.legs[2].foot_init_z;
    Dog.legs[3].foot_init_x = Dog.legs[3].foot_target_x - Dog.legs[3].foot_init_x;
    Dog.legs[3].foot_init_y = Dog.legs[3].foot_target_y - Dog.legs[3].foot_init_y;
    Dog.legs[3].foot_init_z = Dog.legs[3].foot_target_z - Dog.legs[3].foot_init_z;
    uprintf("Dog_Gait_ReInit\n\r");
    Dog_Gait_Reset();
}

void Dog_Gait_Turn()
{
    Dog.legs[0].foot_init_y = LEG0_INIT_POS_Y;
    Dog.legs[1].foot_init_y = LEG1_INIT_POS_Y;
    Dog.legs[2].foot_init_y = LEG2_INIT_POS_Y;
    Dog.legs[3].foot_init_y = LEG3_INIT_POS_Y;
    Dog.legs[0].foot_init_y += delta_dh[0];
    Dog.legs[1].foot_init_y += delta_dh[1];
    Dog.legs[2].foot_init_y += delta_dh[2];
    Dog.legs[3].foot_init_y += delta_dh[3];
    // Dog.ctrl_mode = DOG_MODE_RESET;
}

void Dog_Balance()
{
    float R = -PID_Release(&RollPID, Dog.target_roll, Dog.now_roll); // TODO 换成更好的PID或者别的控制方式
    float P = -PID_Release(&RollPID, Dog.target_pitch, Dog.now_pitch);
    float Y = 0; // Dog.target_yaw;
    uprintf("P %.4f target_P %.4f now_P%.4f | ", P, Dog.target_pitch, Dog.now_pitch);
    uprintf("R %.4f target_R %.4f now_R%.4f \n\r", R, Dog.target_roll, Dog.now_roll);
    float dx = Dog.target_x;
    float dy = Dog.target_y;
    float dz = Dog.target_z;

    float xa = 0;
    float ya = 0;
    float za = 0;
    for (int i = 0; i < 4; i++)
    {

        xa = Dog.legs[i].foot_init_x + delta_x;
        ya = Dog.legs[i].foot_init_y;
        za = Dog.legs[i].foot_init_z;

        Dog.legs[i].foot_target_x = dz * cos(P) * sin(Y) - dx * cos(R) * cos(Y) - dy * cos(Y) * sin(R) + xa * cos(R) * cos(Y) - za * cos(P) * sin(Y) + ya * cos(Y) * sin(R) - dy * cos(R) * sin(P) * sin(Y) + dx * sin(P) * sin(R) * sin(Y) + ya * cos(R) * sin(P) * sin(Y) - xa * sin(P) * sin(R) * sin(Y);
        Dog.legs[i].foot_target_y = za * sin(P) - dz * sin(P) - dy * cos(P) * cos(R) + dx * cos(P) * sin(R) + ya * cos(P) * cos(R) - xa * cos(P) * sin(R);
        Dog.legs[i].foot_target_z = za * cos(P) * cos(Y) - dx * cos(R) * sin(Y) - dz * cos(P) * cos(Y) - dy * sin(R) * sin(Y) + xa * cos(R) * sin(Y) + ya * sin(R) * sin(Y) - dx * cos(Y) * sin(P) * sin(R) - ya * cos(R) * cos(Y) * sin(P) + xa * cos(Y) * sin(P) * sin(R) + dy * cos(R) * cos(Y) * sin(P);
    }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Pose_Adjust()
{
    // float px0 = Dog.legs[0].foot_target_x;
    // float py0 = Dog.legs[0].foot_target_y;
    // float pz0 = Dog.legs[0].foot_target_z;
    // float px1 = Dog.legs[1].foot_target_x;
    // float py1 = Dog.legs[1].foot_target_y;
    // float pz1 = Dog.legs[1].foot_target_z;
    // float px2 = Dog.legs[2].foot_target_x;
    // float py2 = Dog.legs[2].foot_target_y;
    // float pz2 = Dog.legs[2].foot_target_z;
    // float px3 = Dog.legs[3].foot_target_x;
    // float py3 = Dog.legs[3].foot_target_y;
    // float pz3 = Dog.legs[3].foot_target_z;
    // px0 = px0 - Dog.now_x;
    // px1 = px1 - Dog.now_x;
    // px2 = px2 - Dog.now_x;
    // px3 = px3 - Dog.now_x;
    // py0 = py0 - Dog.now_y;
    // py1 = py1 - Dog.now_y;
    // py2 = py2 - Dog.now_y;
    // py3 = py3 - Dog.now_y;
    // pz0 = pz0 - Dog.now_z;
    // pz1 = pz1 - Dog.now_z;
    // pz2 = pz2 - Dog.now_z;
    // pz3 = pz3 - Dog.now_z;
    // Dog.legs[0].foot_target_x = Dog.legs[0].foot_target_x - Dog.now_x;
    // Dog.legs[0].foot_target_y = Dog.legs[0].foot_target_y - Dog.now_y;
    // Dog.legs[0].foot_target_z = Dog.legs[0].foot_target_z - Dog.now_z;
    // Dog.legs[1].foot_target_x = Dog.legs[1].foot_target_x - Dog.now_x;
    // Dog.legs[1].foot_target_y = Dog.legs[1].foot_target_y - Dog.now_y;
    // Dog.legs[1].foot_target_z = Dog.legs[1].foot_target_z - Dog.now_z;
    // Dog.legs[2].foot_target_x = Dog.legs[2].foot_target_x - Dog.now_x;
    // Dog.legs[2].foot_target_y = Dog.legs[2].foot_target_y - Dog.now_y;
    // Dog.legs[2].foot_target_z = Dog.legs[2].foot_target_z - Dog.now_z;
    // Dog.legs[3].foot_target_x = Dog.legs[3].foot_target_x - Dog.now_x;
    // Dog.legs[3].foot_target_y = Dog.legs[3].foot_target_y - Dog.now_y;
    // Dog.legs[3].foot_target_z = Dog.legs[3].foot_target_z - Dog.now_z;

    float R = Dog.target_roll;
    float P = Dog.target_pitch;
    float Y = Dog.target_yaw;

    float dx = Dog.target_x;
    float dy = Dog.target_y;
    float dz = Dog.target_z;

    float xa = 0;
    float ya = 0;
    float za = 0;
    for (int i = 0; i < 4; i++)
    {
        // if (Dog.pose_mode == 1)
        // {
        //     xa = Dog.legs[i].foot_target_x;
        //     ya = Dog.legs[i].foot_target_y;
        //     za = Dog.legs[i].foot_target_z;
        // }
        // if (Dog.pose_mode == 2)
        // {
        xa = Dog.legs[i].foot_init_x;
        ya = Dog.legs[i].foot_init_y;
        za = Dog.legs[i].foot_init_z;
        // }

        Dog.legs[i].foot_target_x = dz * cos(P) * sin(Y) - dx * cos(R) * cos(Y) - dy * cos(Y) * sin(R) + xa * cos(R) * cos(Y) - za * cos(P) * sin(Y) + ya * cos(Y) * sin(R) - dy * cos(R) * sin(P) * sin(Y) + dx * sin(P) * sin(R) * sin(Y) + ya * cos(R) * sin(P) * sin(Y) - xa * sin(P) * sin(R) * sin(Y);
        Dog.legs[i].foot_target_y = za * sin(P) - dz * sin(P) - dy * cos(P) * cos(R) + dx * cos(P) * sin(R) + ya * cos(P) * cos(R) - xa * cos(P) * sin(R);
        Dog.legs[i].foot_target_z = za * cos(P) * cos(Y) - dx * cos(R) * sin(Y) - dz * cos(P) * cos(Y) - dy * sin(R) * sin(Y) + xa * cos(R) * sin(Y) + ya * sin(R) * sin(Y) - dx * cos(Y) * sin(P) * sin(R) - ya * cos(R) * cos(Y) * sin(P) + xa * cos(Y) * sin(P) * sin(R) + dy * cos(R) * cos(Y) * sin(P);
    }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Leg_Point_test(int leg, float target_x, float target_y, float target_z)
{
    float T = GAIT_T;
    float H = GAIT_H;
    get_tick(&Dog_time);
    float time = fmod((Dog_time.s), T);

    float dir = atan2(target_x - Dog.legs[leg].foot_target_x, target_z - Dog.legs[leg].foot_target_z);
    float S = sqrt(pow(target_x - Dog.legs[leg].foot_target_x, 2) + pow(target_z - Dog.legs[leg].foot_target_z, 2));

    Dog.legs[leg].foot_target_x = Dog.legs[leg].foot_init_x + S * (time / T - 1 / (2 * PI) * sin(2 * PI * time / T)) * sin(dir);
    Dog.legs[leg].foot_target_y = Dog.legs[leg].foot_init_y + H * (SGN(T / 2 - time) * (2 * (time / T - 1 / (4 * PI) * sin(4 * PI * time / T)) - 1) + 1);
    Dog.legs[leg].foot_target_z = Dog.legs[leg].foot_init_z + S * (time / T - 1 / (2 * PI) * sin(2 * PI * time / T)) * cos(dir);
    if (S < 0.01)
    {
        Dog.ctrl_mode = DOG_MODE_NONE;
    }

    Dog_Inverse_kinematics(&Dog.legs[leg]);
    Dog_servo_angle_pos();
}

/**
 * @brief 原地转向
 */
void Dog_Gait_Turn_in_place()
{
    float Y = Dog.target_yaw;
    float xa = 0;
    float ya = 0;
    float za = 0;
    float xb = 0;
    float yb = 0;
    float zb = 0;
    float S[4] = {0, 0, 0, 0};
    float dir[4] = {0, 0, 0, 0};
    for (int i = 0; i < 4; i++)
    {
        xa = Dog.legs[i].foot_init_x;
        ya = Dog.legs[i].foot_init_y;
        za = Dog.legs[i].foot_init_z;

        xb = xa * cos(Y) - za * sin(Y);
        yb = ya;
        zb = za * cos(Y) + xa * sin(Y);

        S[i] = sqrt(pow(xb - xa, 2) + pow(zb - za, 2));
        dir[i] = atan2(xb - xa, zb - za);
    }
    float T = GAIT_T;
    get_tick(&Dog_time);
    float time = fmod((Dog_time.s), T);
    float t = 0;
    if (time < T / 5)
    {
    }
    else if (time < 2 * T / 5)
    {
    }
    else if (time < 3 * T / 5)
    {
    }
    else if (time < 4 * T / 5)
    {
    }
    else if (time < T)
    {
    }
}

/**
 * @brief 直走
 */
void Dog_Gait_Walk()
{
    float T = GAIT_T;
    float Tm = T / 2;
    float S = GAIT_S;
    float H = GAIT_H;

    get_tick(&Dog_time);
    float time = fmod((Dog_time.s), T);
    float t = 0;

    for (int i = 0; i < 2; i++)
    {
        t = fmod(time + i * Tm, T);
        if (t < Tm)
        {
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x;
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y + H * (SGN(Tm / 2 - t) * (2 * (t / Tm - 1 / (4 * PI) * sin(4 * PI * t / Tm)) - 1) + 1);
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * (t / Tm - 1 / (2 * PI) * sin(2 * PI * t / Tm));
            Dog.legs[i + 2].foot_target_x = Dog.legs[i + 2].foot_init_x + delta_x;
            Dog.legs[i + 2].foot_target_y = Dog.legs[i + 2].foot_init_y + H * (SGN(Tm / 2 - t) * (2 * (t / Tm - 1 / (4 * PI) * sin(4 * PI * t / Tm)) - 1) + 1);
            Dog.legs[i + 2].foot_target_z = Dog.legs[i + 2].foot_init_z + delta_z + S * (t / Tm - 1 / (2 * PI) * sin(2 * PI * t / Tm));
        }
        else
        {
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x;
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * ((2 * Tm - t) / Tm + 1 / (2 * PI) * sin(2 * PI * t / Tm));
            Dog.legs[i + 2].foot_target_x = Dog.legs[i + 2].foot_init_x + delta_x;
            Dog.legs[i + 2].foot_target_y = Dog.legs[i + 2].foot_init_y;
            Dog.legs[i + 2].foot_target_z = Dog.legs[i + 2].foot_init_z + delta_z + S * ((2 * Tm - t) / Tm + 1 / (2 * PI) * sin(2 * PI * t / Tm));
        }
    }

    // if (time < T * 0.5)
    // {
    //     Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x;
    //     Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + H * (SGN(Tm / 2 - time) * (2 * (time / Tm - 1 / (4 * PI) * sin(4 * PI * time / Tm)) - 1) + 1);
    //     Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z + S * (time / Tm - 1 / (2 * PI) * sin(2 * PI * time / Tm));
    //     Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x;
    //     Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
    //     Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z + S * ((2 * Tm - (time + 0.5 * T)) / Tm + 1 / 2 * PI * sin(2 * PI * (time + 0.5 * T) / Tm));
    //     Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x;
    //     Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y + H * (SGN(Tm / 2 - time) * (2 * (time / Tm - 1 / (4 * PI) * sin(4 * PI * time / Tm)) - 1) + 1);
    //     Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z + S * (time / Tm - 1 / (2 * PI) * sin(2 * PI * time / Tm));
    //     Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x;
    //     Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
    //     Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z + S * ((2 * Tm - (time + 0.5 * T)) / Tm + 1 / 2 * PI * sin(2 * PI * (time + 0.5 * T) / Tm));
    // }
    // else
    // {
    //     Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x;
    //     Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
    //     Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z + S * ((2 * Tm - time) / Tm + 1 / 2 * PI * sin(2 * PI * time / Tm));
    //     Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x;
    //     Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + H * (SGN(Tm / 2 - (time - 0.5 * T)) * (2 * ((time - 0.5 * T) / Tm - 1 / (4 * PI) * sin(4 * PI * (time - 0.5 * T) / Tm)) - 1) + 1);
    //     Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z + S * ((time - 0.5 * T) / Tm - 1 / (2 * PI) * sin(2 * PI * (time - 0.5 * T) / Tm));
    //     Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x;
    //     Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
    //     Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z + S * ((2 * Tm - time) / Tm + 1 / 2 * PI * sin(2 * PI * time / Tm));
    //     Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x;
    //     Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y + H * (SGN(Tm / 2 - (time - 0.5 * T)) * (2 * ((time - 0.5 * T) / Tm - 1 / (4 * PI) * sin(4 * PI * (time - 0.5 * T) / Tm)) - 1) + 1);
    //     Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z + S * ((time - 0.5 * T) / Tm - 1 / (2 * PI) * sin(2 * PI * (time - 0.5 * T) / Tm));
    // }

    // Dog.target_pitch = 0.15;
    // float R = -PID_Release(&RollPID, Dog.target_roll, Dog.now_roll); // TODO 换成更好的PID或者别的控制方式
    // float P = -PID_Release(&RollPID, Dog.target_pitch, Dog.now_pitch);
    // float Y = 0; // Dog.target_yaw;
    // // uprintf("P %.4f target_P %.4f now_P%.4f | ", P, Dog.target_pitch, Dog.now_pitch);
    // // uprintf("R %.4f target_R %.4f now_R%.4f \n\r", R, Dog.target_roll, Dog.now_roll);
    // float dx = Dog.target_x;
    // float dy = Dog.target_y;
    // float dz = Dog.target_z;
    // float xa = 0;
    // float ya = 0;
    // float za = 0;
    // for (int i = 0; i < 4; i++)
    // {
    //     xa = Dog.legs[i].foot_target_x;
    //     ya = Dog.legs[i].foot_target_y;
    //     za = Dog.legs[i].foot_target_z;
    //     Dog.legs[i].foot_target_x = dz * cos(P) * sin(Y) - dx * cos(R) * cos(Y) - dy * cos(Y) * sin(R) + xa * cos(R) * cos(Y) - za * cos(P) * sin(Y) + ya * cos(Y) * sin(R) - dy * cos(R) * sin(P) * sin(Y) + dx * sin(P) * sin(R) * sin(Y) + ya * cos(R) * sin(P) * sin(Y) - xa * sin(P) * sin(R) * sin(Y);
    //     Dog.legs[i].foot_target_y = za * sin(P) - dz * sin(P) - dy * cos(P) * cos(R) + dx * cos(P) * sin(R) + ya * cos(P) * cos(R) - xa * cos(P) * sin(R);
    //     Dog.legs[i].foot_target_z = za * cos(P) * cos(Y) - dx * cos(R) * sin(Y) - dz * cos(P) * cos(Y) - dy * sin(R) * sin(Y) + xa * cos(R) * sin(Y) + ya * sin(R) * sin(Y) - dx * cos(Y) * sin(P) * sin(R) - ya * cos(R) * cos(Y) * sin(P) + xa * cos(Y) * sin(P) * sin(R) + dy * cos(R) * cos(Y) * sin(P);
    //     Dog.legs[i].foot_target_x = 0.2 * Dog.legs[i].foot_target_x + 0.8 * xa;
    //     Dog.legs[i].foot_target_y = 0.2 * Dog.legs[i].foot_target_y + 0.8 * ya;
    //     Dog.legs[i].foot_target_z = 0.2 * Dog.legs[i].foot_target_z + 0.8 * za;
    // }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

/**
 * @brief 全向直走
 */
void Dog_Gait_Walk_Omni()
{
    float T = GAIT_T;
    float Tm = T / 2;
    float S = GAIT_S;
    float H = GAIT_H;

    get_tick(&Dog_time);
    float time = fmod((Dog_time.s), T);
    float t = 0;

    float v_theta = atan2(-Dog.v_x, Dog.v_z);
    for (int i = 0; i < 2; i++)
    {
        t = fmod(time + i * Tm, T);
        if (t < Tm)
        {
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x + S * (t / Tm - 1 / (2 * PI) * sin(2 * PI * t / Tm)) * sin(v_theta);
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y + H * (SGN(Tm / 2 - t) * (2 * (t / Tm - 1 / (4 * PI) * sin(4 * PI * t / Tm)) - 1) + 1);
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * (t / Tm - 1 / (2 * PI) * sin(2 * PI * t / Tm)) * cos(v_theta);
            Dog.legs[i + 2].foot_target_x = Dog.legs[i + 2].foot_init_x + delta_x + S * (t / Tm - 1 / (2 * PI) * sin(2 * PI * t / Tm)) * sin(v_theta);
            Dog.legs[i + 2].foot_target_y = Dog.legs[i + 2].foot_init_y + H * (SGN(Tm / 2 - t) * (2 * (t / Tm - 1 / (4 * PI) * sin(4 * PI * t / Tm)) - 1) + 1);
            Dog.legs[i + 2].foot_target_z = Dog.legs[i + 2].foot_init_z + delta_z + S * (t / Tm - 1 / (2 * PI) * sin(2 * PI * t / Tm)) * cos(v_theta);
        }
        else
        {
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x + S * ((2 * Tm - t) / Tm + 1 / 2 * PI * sin(2 * PI * t / Tm)) * sin(v_theta);
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * ((2 * Tm - t) / Tm + 1 / 2 * PI * sin(2 * PI * t / Tm)) * cos(v_theta);
            Dog.legs[i + 2].foot_target_x = Dog.legs[i + 2].foot_init_x + delta_x + S * ((2 * Tm - t) / Tm + 1 / 2 * PI * sin(2 * PI * t / Tm)) * sin(v_theta);
            Dog.legs[i + 2].foot_target_y = Dog.legs[i + 2].foot_init_y;
            Dog.legs[i + 2].foot_target_z = Dog.legs[i + 2].foot_init_z + delta_z + S * ((2 * Tm - t) / Tm + 1 / 2 * PI * sin(2 * PI * t / Tm)) * cos(v_theta);
        }
    }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Gait_swing()
{
    float T = GAIT_T;

    get_tick(&Dog_time);
    float time = fmod((Dog_time.s), T);

    Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x / 2 * (1 - cos(time * PI / (T / 2)));
    Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
    Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z / 2 * (1 - cos(time * PI / (T / 2)));
    Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x / 2 * (1 - cos(time * PI / (T / 2)));
    Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
    Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z / 2 * (1 - cos(time * PI / (T / 2)));
    Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x / 2 * (1 - cos(time * PI / (T / 2)));
    Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
    Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z / 2 * (1 - cos(time * PI / (T / 2)));
    Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x / 2 * (1 - cos(time * PI / (T / 2)));
    Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
    Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z / 2 * (1 - cos(time * PI / (T / 2)));

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

/**
 * @brief 3+1步态
 */
void Dog_Gait_Pace()
{
    float T = GAIT_T;
    float Tm = T / 4;
    float Tmm = Tm / 2;
    float S = GAIT_S;
    float H = GAIT_H;
    /*
    0->2 x- z-
    1->3 x+ z-
    2->0 x+ z+
    3->1 x- z+
    */

    get_tick(&Dog_time);
    float time = fmod((Dog_time.s), T);
    float gait_delta_x_t = 0;
    float gait_delta_z_t = 0;
    float t = 0;
    int up = 0;
    for (up; up < 4; up++)
    {
        if (fmod(time + up * Tm, T) < Tm)
        {
            uprintf("up : %d\n\r", up);
            up = Dog.phase[up]; // TODO 任意相位关系不对
            break;
        }
    }
    for (int j = 0; j < 4; j++)
    {
        int i = Dog.phase[j];

        t = fmod(time + j * Tm, T);
        if (t < Tm)
        {
            // uprintf("up:%d\n", i);
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x;
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y + H * (SGN(Tm / 2 - t) * (2 * (t / Tm - 1 / (4 * PI) * sin(4 * PI * t / Tm)) - 1) + 1);
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI));
            // uprintf("leg[%d] %.4f %.4f %.4f \n\r", i, Dog.legs[i].foot_target_x, Dog.legs[i].foot_target_y, Dog.legs[i].foot_target_z);
        }
        // if (t < Tmm)
        // {
        //     Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x+delta_x + gait_delta_x_t;
        //     Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y + H * (SGN(Tm / 2 - t) * (2 * (t / Tm - 1 / (4 * PI) * sin(4 * PI * t / Tm)) - 1) + 1);
        //     Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + gait_delta_z_t + delta_z + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI));
        // }
        // else if (Tmm < t && t < Tm)
        // {
        //     Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x+delta_x + gait_delta_x_t;
        //     Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y + H * (SGN(Tm / 2 - t) * (2 * (t / Tm - 1 / (4 * PI) * sin(4 * PI * t / Tm)) - 1) + 1);
        //     Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + gait_delta_z_t + delta_z + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI));
        // }
        else if (t < T / 2)
        {
            gait_delta_x_t = phase_graph[(up + 2) % 4][0] * gait_delta_x;
            gait_delta_z_t = phase_graph[(up + 2) % 4][1] * gait_delta_z;
            // uprintf("i %d j %d up %d (%.1f %.1f)\n\r", i, j, up, gait_delta_x_t, gait_delta_z_t);
            // gait_delta_x_t = phase_graph[(i + 1) % 4][0] * gait_delta_x;
            // gait_delta_z_t = phase_graph[(i + 1) % 4][1] * gait_delta_z;
            // switch (i)
            // {
            // case 0:
            //     gait_delta_x_t = -gait_delta_x;
            //     gait_delta_z_t = gait_delta_z;
            //     break;
            // case 1:
            //     gait_delta_x_t = -gait_delta_x;
            //     gait_delta_z_t = -gait_delta_z;
            //     break;
            // case 2:
            //     gait_delta_x_t = gait_delta_x;
            //     gait_delta_z_t = -gait_delta_z;
            //     break;
            // case 3:
            //     gait_delta_x_t = gait_delta_x;
            //     gait_delta_z_t = gait_delta_z;
            //     break;
            // default:
            //     break;
            // }
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x + gait_delta_x_t * sin(4 * PI * t / T);
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
            // Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * (sin((2 * PI * (t / 3 - (4 * Tm) / 3)) / Tm) / (2 * PI) - (t / 3 - (4 * Tm) / 3) / Tm);
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + gait_delta_z_t * sin(4 * PI * t / T) + delta_z - (4 * S) * t / (3 * T) + 4 * S / 3;
        }
        else if (t < 3 * T / 4)
        {
            gait_delta_x_t = phase_graph[up][0] * gait_delta_x;
            gait_delta_z_t = phase_graph[up][1] * gait_delta_z;
            // uprintf("i %d j %d up %d (%.1f %.1f)\n\r", i, j, up, gait_delta_x_t, gait_delta_z_t);
            // gait_delta_x_t = phase_graph[(i + 2) % 4][0] * gait_delta_x;
            // gait_delta_z_t = phase_graph[(i + 2) % 4][1] * gait_delta_z;
            // switch (i)
            // {
            // case 0:
            //     gait_delta_x_t = -gait_delta_x;
            //     gait_delta_z_t = -gait_delta_z;
            //     break;
            // case 1:
            //     gait_delta_x_t = gait_delta_x;
            //     gait_delta_z_t = -gait_delta_z;
            //     break;
            // case 2:
            //     gait_delta_x_t = gait_delta_x;
            //     gait_delta_z_t = gait_delta_z;
            //     break;
            // case 3:
            //     gait_delta_x_t = -gait_delta_x;
            //     gait_delta_z_t = gait_delta_z;
            //     break;
            // default:
            //     break;
            // }
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x + gait_delta_x_t * sin(4 * PI * t / T);
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
            // Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * (sin((2 * PI * (t / 3 - (4 * Tm) / 3)) / Tm) / (2 * PI) - (t / 3 - (4 * Tm) / 3) / Tm);
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + gait_delta_z_t * sin(4 * PI * t / T) + delta_z - (4 * S) * t / (3 * T) + 4 * S / 3;
        }
        else
        {
            gait_delta_x_t = phase_graph[(up + 2) % 4][0] * gait_delta_x;
            gait_delta_z_t = phase_graph[(up + 2) % 4][1] * gait_delta_z;
            // uprintf("i %d j %d up %d (%.1f %.1f)\n\r", i, j, up, gait_delta_x_t, gait_delta_z_t);
            // gait_delta_x_t = phase_graph[(i + 3) % 4][0] * gait_delta_x;
            // gait_delta_z_t = phase_graph[(i + 3) % 4][1] * gait_delta_z;
            // switch (i)
            // {
            // case 0:
            //     gait_delta_x_t = gait_delta_x;
            //     gait_delta_z_t = -gait_delta_z;
            //     break;
            // case 1:
            //     gait_delta_x_t = gait_delta_x;
            //     gait_delta_z_t = gait_delta_z;
            //     break;
            // case 2:
            //     gait_delta_x_t = -gait_delta_x;
            //     gait_delta_z_t = gait_delta_z;
            //     break;
            // case 3:
            //     gait_delta_x_t = -gait_delta_x;
            //     gait_delta_z_t = -gait_delta_z;
            //     break;
            // default:
            //     break;
            // }
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x + gait_delta_x_t * sin(4 * PI * t / T);
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
            // Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * (sin((2 * PI * (t / 3 - (4 * Tm) / 3)) / Tm) / (2 * PI) - (t / 3 - (4 * Tm) / 3) / Tm);
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + gait_delta_z_t * sin(4 * PI * t / T) + delta_z - (4 * S) * t / (3 * T) + 4 * S / 3;
        }
        // uprintf("leg[%d] %.4f %.4f %.4f \n\r", i, Dog.legs[i].foot_target_x, Dog.legs[i].foot_target_y, Dog.legs[i].foot_target_z);
    }
    // if (t < Tmm)
    // {
    //     Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x+delta_x;
    //     Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + H * (SGN(Tm / 2 - t) * (2 * (t / Tm - 1 / (4 * PI) * sin(4 * PI * t / Tm)) - 1) + 1);
    //     Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z + S * (t / Tm - 1 / (2 * PI) * sin(2 * PI * t / Tm));
    //     Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x+delta_x;
    //     Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
    //     Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z;
    //     Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x+delta_x;
    //     Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
    //     Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z;
    //     Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x+delta_x;
    //     Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
    //     Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z;
    // }
    // else if (Tmm < t && t < Tm)
    // {
    //     Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x+delta_x;
    //     Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
    //     Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z + S * (t / Tm - 1 / (2 * PI) * sin(2 * PI * t / Tm));
    //     Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x+delta_x;
    //     Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
    //     Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z;
    //     Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x+delta_x;
    //     Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
    //     Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z;
    //     Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x+delta_x;
    //     Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
    //     Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z;
    // }
    // else
    // {
    //     Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x+delta_x;
    //     Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
    //     Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z + S * (sin((2 * PI * (t / 3 - (4 * Tm) / 3)) / Tm) / (2 * PI) - (t / 3 - (4 * Tm) / 3) / Tm);
    //     Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x+delta_x;
    //     Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
    //     Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z;
    //     Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x+delta_x;
    //     Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
    //     Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z;
    //     Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x+delta_x;
    //     Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
    //     Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z;
    // }
    // Dog.target_pitch = 0.15;
    // float R = -PID_Release(&RollPID, Dog.target_roll, Dog.now_roll); // TODO 换成更好的PID或者别的控制方式
    // float P = -PID_Release(&RollPID, Dog.target_pitch, Dog.now_pitch);
    // float Y = 0; // Dog.target_yaw;
    // // uprintf("P %.4f target_P %.4f now_P%.4f | ", P, Dog.target_pitch, Dog.now_pitch);
    // // uprintf("R %.4f target_R %.4f now_R%.4f \n\r", R, Dog.target_roll, Dog.now_roll);
    // float dx = Dog.target_x;
    // float dy = Dog.target_y;
    // float dz = Dog.target_z;
    // float xa = 0;
    // float ya = 0;
    // float za = 0;
    // for (int i = 0; i < 4; i++)
    // {
    //     xa = Dog.legs[i].foot_target_x;
    //     ya = Dog.legs[i].foot_target_y;
    //     za = Dog.legs[i].foot_target_z;
    //     Dog.legs[i].foot_target_x = dz * cos(P) * sin(Y) - dx * cos(R) * cos(Y) - dy * cos(Y) * sin(R) + xa * cos(R) * cos(Y) - za * cos(P) * sin(Y) + ya * cos(Y) * sin(R) - dy * cos(R) * sin(P) * sin(Y) + dx * sin(P) * sin(R) * sin(Y) + ya * cos(R) * sin(P) * sin(Y) - xa * sin(P) * sin(R) * sin(Y);
    //     Dog.legs[i].foot_target_y = za * sin(P) - dz * sin(P) - dy * cos(P) * cos(R) + dx * cos(P) * sin(R) + ya * cos(P) * cos(R) - xa * cos(P) * sin(R);
    //     Dog.legs[i].foot_target_z = za * cos(P) * cos(Y) - dx * cos(R) * sin(Y) - dz * cos(P) * cos(Y) - dy * sin(R) * sin(Y) + xa * cos(R) * sin(Y) + ya * sin(R) * sin(Y) - dx * cos(Y) * sin(P) * sin(R) - ya * cos(R) * cos(Y) * sin(P) + xa * cos(Y) * sin(P) * sin(R) + dy * cos(R) * cos(Y) * sin(P);
    //     Dog.legs[i].foot_target_x = 0.2 * Dog.legs[i].foot_target_x + 0.8 * xa;
    //     Dog.legs[i].foot_target_y = 0.2 * Dog.legs[i].foot_target_y + 0.8 * ya;
    //     Dog.legs[i].foot_target_z = 0.2 * Dog.legs[i].foot_target_z + 0.8 * za;
    // }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

/**
 * @brief 全向3+1步态
 */
void Dog_Gait_Pace_Omni()
{
    float T = GAIT_T;
    float Tm = T / 4;
    float Tmm = Tm / 2;
    float S = GAIT_S;
    float H = GAIT_H;
    /*
    0->2 x- z-
    1->3 x+ z-
    2->0 x+ z+
    3->1 x- z+
    */

    get_tick(&Dog_time);
    float time = fmod((Dog_time.s), T);
    float gait_delta_x_t = 0;
    float gait_delta_z_t = 0;
    float t = 0;

    for (int i = 0; i < 4; i++)
    {
        t = fmod(time + i * Tm, T);

        if (t < Tm)
        {
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) * sin(Dog.target_yaw);
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y + H * (SGN(Tm / 2 - t) * (2 * (t / Tm - 1 / (4 * PI) * sin(4 * PI * t / Tm)) - 1) + 1);
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) * cos(Dog.target_yaw);
        }

        else if (t < T / 2)
        {
            switch (i)
            {
            case 0:
                gait_delta_x_t = -gait_delta_x;
                gait_delta_z_t = gait_delta_z;
                break;
            case 1:
                gait_delta_x_t = -gait_delta_x;
                gait_delta_z_t = -gait_delta_z;
                break;
            case 2:
                gait_delta_x_t = gait_delta_x;
                gait_delta_z_t = -gait_delta_z;
                break;
            case 3:
                gait_delta_x_t = gait_delta_x;
                gait_delta_z_t = gait_delta_z;
                break;
            default:
                break;
            }
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x + gait_delta_x_t * sin(4 * PI * t / T) - ((4 * S) * t / (3 * T) + 4 * S / 3) * sin(Dog.target_yaw);
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
            // Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * (sin((2 * PI * (t / 3 - (4 * Tm) / 3)) / Tm) / (2 * PI) - (t / 3 - (4 * Tm) / 3) / Tm);
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + gait_delta_z_t * sin(4 * PI * t / T) + delta_z - ((4 * S) * t / (3 * T) + 4 * S / 3) * cos(Dog.target_yaw);
        }
        else if (t < 3 * T / 4)
        {
            switch (i)
            {
            case 0:
                gait_delta_x_t = -gait_delta_x;
                gait_delta_z_t = -gait_delta_z;
                break;
            case 1:
                gait_delta_x_t = gait_delta_x;
                gait_delta_z_t = -gait_delta_z;
                break;
            case 2:
                gait_delta_x_t = gait_delta_x;
                gait_delta_z_t = gait_delta_z;
                break;
            case 3:
                gait_delta_x_t = -gait_delta_x;
                gait_delta_z_t = gait_delta_z;
                break;
            default:
                break;
            }
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x + gait_delta_x_t * sin(4 * PI * t / T) - ((4 * S) * t / (3 * T) + 4 * S / 3) * sin(Dog.target_yaw);
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
            // Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * (sin((2 * PI * (t / 3 - (4 * Tm) / 3)) / Tm) / (2 * PI) - (t / 3 - (4 * Tm) / 3) / Tm);
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + gait_delta_z_t * sin(4 * PI * t / T) + delta_z - ((4 * S) * t / (3 * T) + 4 * S / 3) * cos(Dog.target_yaw);
        }
        else
        {
            switch (i)
            {
            case 0:
                gait_delta_x_t = gait_delta_x;
                gait_delta_z_t = -gait_delta_z;
                break;
            case 1:
                gait_delta_x_t = gait_delta_x;
                gait_delta_z_t = gait_delta_z;
                break;
            case 2:
                gait_delta_x_t = -gait_delta_x;
                gait_delta_z_t = gait_delta_z;
                break;
            case 3:
                gait_delta_x_t = -gait_delta_x;
                gait_delta_z_t = -gait_delta_z;
                break;
            default:
                break;
            }
            Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x + delta_x + gait_delta_x_t * sin(4 * PI * t / T) - ((4 * S) * t / (3 * T) + 4 * S / 3) * sin(Dog.target_yaw);
            Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
            // Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + delta_z + S * (sin((2 * PI * (t / 3 - (4 * Tm) / 3)) / Tm) / (2 * PI) - (t / 3 - (4 * Tm) / 3) / Tm);
            Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z + gait_delta_z_t * sin(4 * PI * t / T) + delta_z - ((4 * S) * t / (3 * T) + 4 * S / 3) * cos(Dog.target_yaw);
        }
    }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Gait_Upstair()
{
    float T = GAIT_T;
    float S = GAIT_S;
    float H = GAIT_H*1.7;
    float Tm = T / 6;
    float tmm = Tm / 2;
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s), T);
    static float climb_delta_t = 0;
    if (climb_first_flag == 1)
    {
        climb_delta_t = t - T / 12;
        climb_first_flag = 0;
    }
    else if (climb_first_flag == 2)
    {
        climb_delta_t = 0;
    }
    t = fmod(t - climb_delta_t + T, T);

    static int order = 1;
    static int T_6 = 0;
    //***后移重心***//
    if (t < T / 6)
    {
        if (T_6 == 1)
        {
            T_6 = 0;
            // order = (order + 1) % 2;
        }
        // uprintf("1: %.3f\n\r", t);
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x;
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + delta_h * sin(t * 3 * PI / T);
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z_front * cos(t * 3 * PI / T) - delta_z_back * sin(t * 3 * PI / T) - t * 3 * S / T;
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x;
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + delta_h * sin(t * 3 * PI / T);
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z_front * cos(t * 3 * PI / T) - delta_z_back * sin(t * 3 * PI / T) - t * 3 * S / T;
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x;
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z_front * cos(t * 3 * PI / T) - delta_z_back * sin(t * 3 * PI / T) - t * 3 * S / T + S / 2;
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x;
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z_front * cos(t * 3 * PI / T) - delta_z_back * sin(t * 3 * PI / T) - t * 3 * S / T + S / 2;
    }
    //***抬左前***//
    else if (t < 2 * T / 6)
    {
        // uprintf("2: %.3f\n\r", t);
        t = t - T / 6;
        if (order)
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x;
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + delta_h + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z - delta_z_back + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + delta_h;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T) - S / 2;
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
        }
        else
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + delta_h;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T) - S / 2;
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x;
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + delta_h + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z - delta_z_back + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
        }
    }
    //***抬右前***//
    else if (t < 3 * T / 6)
    {
        t = t - 2 * T / 6;
        if (order)
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + delta_h;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T) + S / 2;
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x;
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + delta_h + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z - delta_z_back + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
        }
        else
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x;
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + delta_h + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z - delta_z_back + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + delta_h;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T) + S / 2;
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
        }
        // uprintf("3: %.3f\n\r", t);
    }
    //***前移重心***//
    else if (t < 4 * T / 6)
    {

        // uprintf("4: %.3f\n\r", t);
        t = t - T / 2;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x;
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + delta_h - delta_h * sin(t * 3 * PI / T);
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z - delta_z_back * cos(t * 3 * PI / T) + delta_z_front * sin(t * 3 * PI / T) - t * 3 * S / T + S / 2;
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x;
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + delta_h - delta_h * sin(t * 3 * PI / T);
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z - delta_z_back * cos(t * 3 * PI / T) + delta_z_front * sin(t * 3 * PI / T) - t * 3 * S / T + S / 2;
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x;
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z - delta_z_back * cos(t * 3 * PI / T) + delta_z_front * sin(t * 3 * PI / T) - t * 3 * S / T;
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x;
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z - delta_z_back * cos(t * 3 * PI / T) + delta_z_front * sin(t * 3 * PI / T) - t * 3 * S / T;
    }
    //***抬左后***//
    else if (t < 5 * T / 6)
    {
        t = t - 4 * T / 6;
        if (order)
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T) - S / 2;
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x;
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y + 0.5 * H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z_front + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
        }
        else
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x;
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y + 0.5 * H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z_front + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T) - S / 2;
        }
        // uprintf("5: %.3f\n\r", t);
    }
    //***抬右后***//
    else if (t < T)
    {
        t = t - 5 * T / 6;
        if (order)
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x - gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x - gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x;
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y + 0.5 * H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z_front + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x - gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T) + S / 2;
        }
        else
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x + gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x + gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x + gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T) + S / 2;
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x;
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y + 0.5 * H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z_front + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
        }
        T_6 = 1;
        // uprintf("6: %.3f\n\r", t);
    }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Gait_Downstair()
{
    
    float T = GAIT_T;
    float S = GAIT_S;
    float H = GAIT_H;
    float Tm = T / 6;
    float tmm = Tm / 2;
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s), T);
    static float climb_delta_t = 0;
    if (climb_first_flag == 1)
    {
        climb_delta_t = t - T / 12;
        climb_first_flag = 0;
    }
    else if (climb_first_flag == 2)
    {
        climb_delta_t = 0;
    }
    t = fmod(t - climb_delta_t + T, T);
    uprintf("in downstair %f\n\r",t);
    static int order = 1;
    static int T_6 = 0;
    //***后移重心***//
    if (t < T / 6)
    {
        if (T_6 == 1)
        {
            T_6 = 0;
            // order = (order + 1) % 2;
        }
        // uprintf("1: %.3f\n\r", t);
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x;
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + delta_h * sin(t * 3 * PI / T);
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z_front * cos(t * 3 * PI / T) - delta_z_back * sin(t * 3 * PI / T) - t * 3 * S / T;
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x;
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + delta_h * sin(t * 3 * PI / T);
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z_front * cos(t * 3 * PI / T) - delta_z_back * sin(t * 3 * PI / T) - t * 3 * S / T;
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x;
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z_front * cos(t * 3 * PI / T) - delta_z_back * sin(t * 3 * PI / T) - t * 3 * S / T + S / 2;
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x;
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z_front * cos(t * 3 * PI / T) - delta_z_back * sin(t * 3 * PI / T) - t * 3 * S / T + S / 2;
    }
    //***抬左前***//
    else if (t < 2 * T / 6)
    {
        uprintf("%d\n\r", order);
        t = t - T / 6;
        if (order)
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x;
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y - delta_h + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z - delta_z_back + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y - delta_h;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T) - S / 2;
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
        }
        else
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y - delta_h;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T) - S / 2;
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x;
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y - delta_h + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z - delta_z_back + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
        }
    }
    //***抬右前***//
    else if (t < 3 * T / 6)
    {
        t = t - 2 * T / 6;
        if (order)
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y - delta_h;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T) + S / 2;
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x;
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y - delta_h + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z - delta_z_back + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
        }
        else
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x;
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y - delta_h + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z - delta_z_back + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y - delta_h;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T) + S / 2;
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z - delta_z_back + gait_delta_z * sin(6 * PI * t / T);
        }
        // uprintf("3: %.3f\n\r", t);
    }
    //***前移重心***//
    else if (t < 4 * T / 6)
    {

        // uprintf("4: %.3f\n\r", t);
        t = t - T / 2;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x;
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + delta_h - delta_h * sin(t * 3 * PI / T);
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z - delta_z_back * cos(t * 3 * PI / T) + delta_z_front * sin(t * 3 * PI / T) - t * 3 * S / T + S / 2;
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x;
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + delta_h - delta_h * sin(t * 3 * PI / T);
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z - delta_z_back * cos(t * 3 * PI / T) + delta_z_front * sin(t * 3 * PI / T) - t * 3 * S / T + S / 2;
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x;
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z - delta_z_back * cos(t * 3 * PI / T) + delta_z_front * sin(t * 3 * PI / T) - t * 3 * S / T;
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x;
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z - delta_z_back * cos(t * 3 * PI / T) + delta_z_front * sin(t * 3 * PI / T) - t * 3 * S / T;
    }
    //***抬左后***//
    else if (t < 5 * T / 6)
    {
        t = t - 4 * T / 6;
        if (order)
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x + gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T) - S / 2;
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x;
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z_front + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
        }
        else
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x;
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z_front + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x - gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T) - S / 2;
        }
        // uprintf("5: %.3f\n\r", t);
    }
    //***抬右后***//
    else if (t < T)
    {
        t = t - 5 * T / 6;
        if (order)
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x - gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x - gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x;
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z_front + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x - gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T) + S / 2;
        }
        else
        {
            Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + delta_x + gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
            Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + delta_x + gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
            Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + delta_x + gait_delta_gain * gait_delta_x * sin(6 * PI * t / T);
            Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
            Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + delta_z_front - gait_delta_z * sin(6 * PI * t / T) + S / 2;
            Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + delta_x;
            Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y + H * ((t < tmm ? t : Tm - t) / tmm - 1 / (2 * PI) * sin(2 * PI * (t < tmm ? t : Tm - t) / tmm));
            Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + delta_z_front + S * (t / Tm - sin(2 * PI * t / Tm) / (2 * PI)) - S / 2;
        }
        T_6 = 1;
        // uprintf("6: %.3f\n\r", t);
    }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void angle_init(){
    movecmp_flag=0;
    angle_num=0;
    all_angle=0;
}

void Update_angle(){
    int angle;
    float t = Dog_time.s;
    if(movecmp_flag)
        angle_init();
    angle_num++;
    if(UART3_TaskBuffer[3]==2)
        angle =-1 * UART3_TaskBuffer[4];
    else if(UART3_TaskBuffer[3]==0)
        angle = 0;
    else if(UART3_TaskBuffer[3]==1)
        angle = UART3_TaskBuffer[4];
    angle -= 9;
    all_angle =(all_angle * (angle_num-1) + angle)/angle_num;
}

void Update_error(){
    error_x[0]+=-1*0.0025*all_angle;
    if(error_x[0]<-0.04)error_x[0]=-0.045;
    if(error_x[0]>0.04)error_x[0]=0.045;
}

void Dog_Gait_Update_downstair()
{
    float T = GAIT_T;
    float Tm = (T - 2) / 4;
    float Tmm = Tm / 2;
    float S = GAIT_S;
    float H = GAIT_H;
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s), T);
    int num_x;
    for(int i=0;i<4;i++)
    {
        Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
        Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x;
        Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z;
        Dog.legs[i].foot_target_z += Downstair_fun_z(t);
        Dog.legs[i].foot_target_y += Downstair_fun_y(t,i);
    }
    if(t<1){
        for(int i=0;i<4;i++){
            num_x = i / 2;
            Dog.legs[i].foot_target_x -= error_x[num_x] * cos(PI * t / 2);
        }
    }
    else if((t>=1&&t<=3)||t>=4) {
        if(t>=4)t-=1;
        t-=1;
        int i = t;
        for (int j = 0; j < i; j++) {
            num_x=j/2;
            Dog.legs[j].foot_target_z += S;
            Dog.legs[j].foot_target_y -= 2 * H;
            Dog.legs[j].foot_target_z -= error_x[num_x];
        }
        t = fmod(t, Tm);
        num_x=i/2;
        Dog.legs[i].foot_target_y += H * Downstair_door_fun(t / Tm);
        Dog.legs[i].foot_target_z += S * line_fun(t / Tm);
        Dog.legs[i].foot_target_x -= error_x[num_x] * sin(PI * t / Tm / 2);
    }
    else if(t>3) {
        for (int j = 0; j < 2; j++) {
            num_x=j/2;
            Dog.legs[j].foot_target_z += S;
            Dog.legs[j].foot_target_y -= 2 * H;
            Dog.legs[j].foot_target_z -= error_x[num_x];
        }
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);
    Dog_servo_angle_pos();
}


void Dog_Gait_Update_upstair()
{
    float T = GAIT_T;
    float Tm = (T - 2) / 4;
    float S = 1.5 * GAIT_S;
    float H = GAIT_H;
    int num_x=0;
    int i=0;
    get_tick(&Dog_time);
    error_x[0]=0;
    error_x[1]=0;
    float t = fmod((Dog_time.s), T);
    for(int i=0;i<4;i++)
    {
        Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
        Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x;
        Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z;
        Dog.legs[i].foot_target_z += Upstair_fun_z(t);
        Dog.legs[i].foot_target_y += Upstair_fun_y(t,i);
    }
    if((t>=1&&t<=3)||t>=4) {
        if(t>=4)t-=1;
        t-=1;
        i = t;
        for (int j = 0; j < i; j++) {
            num_x=j/2;
            Dog.legs[j].foot_target_z += S;
        }
        if(i==1)
            Dog.legs[0].foot_target_x -= error_x [0];
        t = fmod(t, Tm);
        num_x=i/2;
        Dog.legs[i].foot_target_y += 2.5 * H * door_fun(t / Tm);
        Dog.legs[i].foot_target_z += S * line_fun(t / Tm);
        Dog.legs[i].foot_target_x -= error_x[num_x] * sin(PI * t / Tm / 2);
    }
    else if(t>3){
        t-=3;
        for (int j = 0; j < 2; j++) {
            num_x=j/2;
            Dog.legs[j].foot_target_z += S;
            Dog.legs[j].foot_target_x -= error_x[num_x] * cos(PI * t / 2);
        }
        for (int j = 2; j < 4; j++) {
            num_x = j / 2;
            Dog.legs[j].foot_target_x += error_x[num_x] * sin(PI * t / 2);
        }
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);
    Dog_servo_angle_pos();
}

void Dog_Gait_Update_walk()
{
    float T = GAIT_T;
    float Tw=T-2;
    float Tm = Tw / 4;
    float S = GAIT_S+0.01;
    float H = GAIT_H;
    int num_x=0;
    get_tick(&Dog_time);
    T-=3;
    float t = fmod((Dog_time.s), T);
    t*=2;
    for(int i=0;i<4;i++)
    {
        Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
        Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x;
        Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z;
        Dog.legs[i].foot_target_z += move_fun(t);
    }
    if(t<=3) {
        if(!movecmp_flag)
        {
            movecmp_flag=1;
            error_x[0] = 0.012;//0.018//0.03平地wallk
            error_x[1] = 0;//0.008
//            Update_error();
//            uprintf("%lf %lf \n",error_x[0],error_x[1]);
        }
        int i = t / Tm;
        int change[4] = {0, 2, 1, 3};
        for (int j = 0; j < i; j++) {
            num_x=change[j]/2;
            Dog.legs[change[j]].foot_target_z += S;
            Dog.legs[change[j]].foot_target_x -= error_x[num_x];
        }
        i = change[i];
        num_x=i/2;
        t = fmod(t, Tm);
        Dog.legs[i].foot_target_y += 2.5 * H * door_fun(t / Tm);
        Dog.legs[i].foot_target_z +=  S * line_fun(t / Tm);
        Dog.legs[i].foot_target_x -= error_x[num_x] * sin(PI * t / Tm / 2);
    }
    else if(t>3&&t<4){
        t-=3;
        int i = 3;
        int change[4] = {0, 2, 1, 3};
        for (int j = 0; j < i; j++) {
            num_x=change[j]/2;
            Dog.legs[change[j]].foot_target_z += S;
            Dog.legs[change[j]].foot_target_x -= error_x[num_x] * cos(PI * t / 2);
        }
        Dog.legs[3].foot_target_x += error_x[1] * sin(PI * t / Tm / 2);
    }
    else if(t>=4&&t<=5){
        t=t-1;
        int i = t/Tm;
        int change[4] = {0, 2, 1, 3};
        for (int j = 0; j < i; j++)
            Dog.legs[change[j]].foot_target_z += S;
        i = change[i];
        t = fmod(t, Tm);
        Dog.legs[i].foot_target_y += 2.5 * H * door_fun(t / Tm);
        Dog.legs[i].foot_target_z +=  S * line_fun(t / Tm);
        Dog.legs[i].foot_target_x += error_x[1] * cos(PI * t / Tm / 2);
    }
    else{
        for (int j = 0; j < 4; j++) {
            Dog.legs[j].foot_target_z += S;
        }
        Update_angle();
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);
    Dog_servo_angle_pos();
}

void Dog_Gait_Test_walk()
{
    float T = GAIT_T;
    float Tw=T-2;
    float Tm = Tw / 4;
    float S = GAIT_S+0.01;
    float H = GAIT_H;
    int num_x=0;
    get_tick(&Dog_time);
    T-=3;
    float t = fmod((Dog_time.s), T);
    t*=2;
    for(int i=0;i<4;i++)
    {
        Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
        Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x;
        Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z;
        Dog.legs[i].foot_target_z += move_fun(t);
    }
    if(t<=2) {
        if(!movecmp_flag)
        {
            movecmp_flag=1;
            error_x[0] = 0.012;//0.018//0.03平地wallk
            error_x[1] = 0;//0.008
            Update_error();
            uprintf("%lf %lf \n",error_x[0],error_x[1]);
        }
        int i = t / Tm;
        for (int j = 0; j < i; j++) {
            num_x=j/2;
            Dog.legs[j].foot_target_z += S;
            Dog.legs[j].foot_target_x -= error_x[num_x];
        }
        num_x=i/2;
        t = fmod(t, Tm);
        Dog.legs[i].foot_target_y += 2.5 * H * door_fun(t / Tm);
        Dog.legs[i].foot_target_z +=  S * line_fun(t / Tm);
        Dog.legs[i].foot_target_x -= error_x[num_x] * sin(PI * t / Tm / 2);
    }
    else if(t>2&&t<3){
        t-=2;
        int i = 2;
        for (int j = 0; j < i; j++) {
            num_x=j/2;
            Dog.legs[j].foot_target_z += S;
            Dog.legs[j].foot_target_x -= error_x[num_x] * cos(PI * t / 2);
        }
        Dog.legs[2].foot_target_x += error_x[1] * sin(PI * t / Tm / 2);
        Dog.legs[3].foot_target_x += error_x[1] * sin(PI * t / Tm / 2);
    }
    else if(t>=3&&t<=5){
        t=t-1;
        int i = t/Tm;
        for (int j = 0; j < i; j++)
            Dog.legs[j].foot_target_z += S;
        t = fmod(t, Tm);
        Dog.legs[i].foot_target_y += 2.5 * H * door_fun(t / Tm);
        Dog.legs[i].foot_target_z +=  S * line_fun(t / Tm);
        Dog.legs[i].foot_target_x += error_x[1] * cos(PI * t / Tm / 2);
        Update_angle();
    }
    else{
        for (int j = 0; j < 4; j++) {
            Dog.legs[j].foot_target_z += S;
        }
        Update_angle();
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);
    Dog_servo_angle_pos();
}

void Dog_Gait_Update_balance() {
    float T = GAIT_T;
    float Tw = T - 2;
    float Tm = Tw / 4;
    float H = GAIT_H;
    int num_x = 0;
    get_tick(&Dog_time);
    T -= 2;
    float t = fmod((Dog_time.s), T);
    t *= 1.5;
    for (int i = 0; i < 4; i++) {
        Dog.legs[i].foot_target_y = Dog.legs[i].foot_init_y;
        Dog.legs[i].foot_target_x = Dog.legs[i].foot_init_x;
        Dog.legs[i].foot_target_z = Dog.legs[i].foot_init_z;
        Dog.legs[i].foot_target_z += move_fun_bal(t);
    }
    if (t < 2) {
        int i = t;
        t = fmod(t, Tm);
        for (int j = 0; j < i; j++) {
            num_x = j / 2;
            Dog.legs[j].foot_target_x -= error_x[num_x];
        }
        Dog.legs[i].foot_target_y += H * door_fun(t);
        Dog.legs[i].foot_target_x -= error_x[num_x] * sin(PI * t / 2);
    }
    else if (t >= 2 && t < 3) {
        t -= 2;
        for (int j = 0; j < 2; j++)
            Dog.legs[j].foot_target_x -= error_x[0] * cos(PI * t / 2);
        for (int j = 2; j <= 3; j++)
            Dog.legs[j].foot_target_x += error_x[1] * sin(PI * t / 2);
    }
    else if (t >= 3 && t <= 5){
        t-=1;
        int i = t;
        t = fmod(t, Tm);
        for (int j = i; j < 4; j++) {
            num_x = j / 2;
            Dog.legs[j].foot_target_x += error_x[num_x];
        }
        Dog.legs[i].foot_target_y += H * door_fun(t / Tm);
        Dog.legs[i].foot_target_x -= error_x[num_x] * sin(PI * t / 2);
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);
    Dog_servo_angle_pos();
}

void Dog_Gait_shangtaijie_zuoyou()
{

    float T = GAIT_T;
    float S = GAIT_S;
    float H = GAIT_H;
    //   float DS = (GAIT_DS + delta_ds);
    //  float Dth = (GAIT_DTH + delta_Dth);
    //  float dth = (GAIT_dth + delta_dth);
    // float dz = (GAIT_DZ + delta_dz);

    get_tick(&Dog_time);
    float t = fmod((Dog_time.s), T);

    float h = 60 / 10 * 1.1;
    float H1 = H - 0.0;               //台阶高0.030
    float dx_10 = barycenter_central; // LL / 6;
    float dx_1 = barycenter_left;     // LL / 3;  //重心左移，系数可调
    float dx_2 = barycenter_right;    // LL / 3;  //重心右移，系数可调
    float dz_10 = barycenter_back;    // 0 * dz; //重心后移，系数可调
    float dz_2 = barycenter_front;    // 3 * dz;  //重心前移，系数可调

    //******抬右前*******//
    if (t < T / 24)
    {
        uprintf("---1---\n\r");
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10)-H / 4 * (1 - cos(t * PI / (T / 48)));
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10);
    }
    else if (t >= T / 24 && t < T / 12)
    {
        uprintf("---2---\n\r");
        t = t - T / 24;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + H;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10);
    }
    else if (t >= T / 12 && t < T / 8)
    {
        uprintf("---3---\n\r");
        t = t - T / 12;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + H + H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10);
    }
    //******移重心*******//
    else if (t >= T / 8 && t < T / 4)
    {
        uprintf("---4---\n\r");
        t = t - T / 8;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + H + H1;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10);
    }
    //******左前*******//
    else if (t >= T / 4 && t < T / 4 + T / 24)
    {
        uprintf("---5---\n\r");
        t = t - T / 4;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10)-H / 4 * (1 - cos(t * PI / (T / 48)));
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + H + H1;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10);
    }
    else if (t >= T / 4 + T / 24 && t < T / 4 + T / 12)
    {
        uprintf("---6---\n\r");
        t = t - T / 4 - T / 24;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + H;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + (S) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + H + H1;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10);
    }
    else if (t >= T / 4 + T / 12 && t < T / 4 + T / 8)
    {
        uprintf("---7---\n\r");
        t = t - T / 4 - T / 12;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + H + H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + H + H1;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10);
    }
    //******移重心*******//
    else if (t >= T / 4 + T / 8 && t < T / 4 + T / 4)
    {
        uprintf("---8---\n\r");
        t = t - T / 4 - T / 8;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y + H + H1 - (-H + H1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y + H + H1 - (-H + H1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    //******左后*******//
    else if (t >= T / 4 + T / 4 && t < T / 4 + T / 4 + T / 24)
    {
        uprintf("---9---\n\r");
        t = t - T / 4 - T / 4;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10) - (dz_2);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10) - (dz_2)-H / 4 * (1 - cos(t * PI / (T / 48)));
    }
    else if (t >= T / 4 + T / 4 + T / 24 && t < T / 4 + T / 4 + T / 12)
    {
        uprintf("---10---\n\r");
        t = t - T / 4 - T / 4 - T / 24;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10) - (dz_2);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y + H;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10) + (S) / 2 * (1 - cos(t * PI / (T / 24))) - (dz_2);
    }
    else if (t >= T / 4 + T / 4 + T / 12 && t < T / 4 + T / 4 + T / 8)
    {
        uprintf("---11---\n\r");
        t = t - T / 4 - T / 4 - T / 12;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10) - (dz_2);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y + H + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24))) - (dz_2);
    }
    //******移重心*******//
    else if (t >= T / 2 + T / 8 && t < T / 2 + T / 4)
    {
        uprintf("---12---\n\r");
        t = t - T / 2 - T / 8;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2) - (dx_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2) - (dx_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2) - (dx_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10) - (dz_2);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2) - (dx_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
    }
    //******左后*******//
    else if (t >= T / 2 + T / 4 && t < T / 2 + T / 4 + T / 24)
    {
        uprintf("---13---\n\r");
        t = t - T / 2 - T / 4;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10)-H / 4 * (1 - cos(t * PI / (T / 48))) - (dz_2);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
    }
    else if (t >= T / 2 + T / 4 + T / 24 && t < T / 2 + T / 4 + T / 12)
    {
        uprintf("---14---\n\r");
        t = t - T / 2 - T / 24 - T / 4;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y + H;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10) + (S) / 2 * (1 - cos(t * PI / (T / 24))) - (dz_2);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
    }
    else if (t >= T / 2 + T / 4 + T / 12 && t < T / 2 + T / 4 + T / 8)
    {
        uprintf("---15---\n\r");
        t = t - T / 2 - T / 4 - T / 12;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y + H + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24))) - (dz_2);
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
    }
    //******移重心*******//
    else
    {
        uprintf("---16---\n\r");
        t = t - T / 2 - T / 4 - T / 8;
        Dog.legs[0].foot_target_x = Dog.legs[0].foot_init_x + (-dx_10) + (dx_2) - (dx_1) - ((dx_2) - (dx_1)) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_y = Dog.legs[0].foot_init_y;
        Dog.legs[0].foot_target_z = Dog.legs[0].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) + ((dz_2)-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = Dog.legs[1].foot_init_x + (-dx_10) + (dx_2) - (dx_1) - ((dx_2) - (dx_1)) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_y = Dog.legs[1].foot_init_y;
        Dog.legs[1].foot_target_z = Dog.legs[1].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) + ((dz_2)-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = Dog.legs[2].foot_init_x + (-dx_10) + (dx_2) - (dx_1) - ((dx_2) - (dx_1)) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_y = Dog.legs[2].foot_init_y;
        Dog.legs[2].foot_target_z = Dog.legs[2].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) + ((dz_2)-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = Dog.legs[3].foot_init_x + (-dx_10) + (dx_2) - (dx_1) - ((dx_2) - (dx_1)) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_y = Dog.legs[3].foot_init_y;
        Dog.legs[3].foot_target_z = Dog.legs[3].foot_init_z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) + ((dz_2)-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 8)));
    }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    switch (GPIO_Pin)
    {
    case KEY1_Pin:
        // Servo_Sync_Torque(1);
        // Dog_Gait_Reset();
        // Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // Servo_Sync_ResetLeg(4);
        Servo_Sync_Torque(1);
        Dog.ctrl_mode = DOG_MODE_WALK;

        break;

    case KEY2_Pin:
        Servo_Sync_Torque(1);
        action_stop();
        // state = state_1_begin;
        state = state_recognize_ball;
        break;

    case S2_Pin:
        if (HAL_GPIO_ReadPin(S2_GPIO_Port, S2_Pin) == GPIO_PIN_RESET)
        {
            Servo_Sync_Torque(1);
            Dog.ctrl_mode = DOG_MODE_NONE;
        }
        else
        {
            Servo_Sync_Torque(0);
            Dog.ctrl_mode = DOG_MODE_NONE;
        }
        break;

    case GPIO_PIN_6:
        break;
    default:
        break;
    }
}

/**
 * @brief 更新舵机目标angle后调用此函数
 */
void Dog_servo_angle_pos()
{
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // Dog.legs[i].servo[j].goal_angle = broadcast_ctrl_angle[i * 3 + j];
            Servo_Angle2Pos_Mappping(&Dog.legs[i].servo[j]);
            broadcast_ctrl_pos[i * 3 + j] = Dog.legs[i].servo[j].goal_pos;
            broadcast_ctrl_angle[i * 3 + j] = Dog.legs[i].servo[j].goal_angle;
        }
    }
}

/**
 * @brief 更新舵机目标pos后调用此函数
 */
void Dog_servo_pos_angle()
{
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // Dog.legs[i].servo[j].goal_pos = broadcast_ctrl_pos[i * 3 + j];
            Servo_Pos2Angle_Mappping(&Dog.legs[i].servo[j]);
            broadcast_ctrl_angle[i * 3 + j] = Dog.legs[i].servo[j].goal_angle;
            broadcast_ctrl_pos[i * 3 + j] = Dog.legs[i].servo[j].goal_pos;
        }
    }
}

/**
 * @brief 单腿逆运动学
 * @param  leg              待计算的腿
 */
void Dog_Inverse_kinematics(Leg_s *leg)
{
    int id = leg->No;
    float px;
    float py;
    float pz;

    px = leg->foot_target_x; //+ LEG0_INIT_POS_X;
    py = leg->foot_target_y; //+ LEG0_INIT_POS_Y;
    pz = leg->foot_target_z; //+ LEG0_INIT_POS_Z;

    float la = LEG_LA;
    float lb = LEG_LB;
    float lc = LEG_LC;
    float l1 = LEG_L1;
    float l2 = LEG_L2;
    float l3 = LEG_L3;
    float l4 = LEG_L4;

    switch (leg->No)
    {
    case 0:
        break;
    case 1:
        la = -LEG_LA;
        l1 = -LEG_L1;
        break;
    case 2:
        la = -LEG_LA;
        l1 = -LEG_L1;
        lb = -LEG_LB;
        break;
    case 3:
        lb = -LEG_LB;
        break;
    default:
        break;
    }

    double th1 = 0;
    double th2_1 = 0;
    double th2_2 = 0;
    double th3_1 = 0;
    double th3_2 = 0;
    //计算th1
    th1 = atan2(sqrt(pow(px - la / 2, 2) + pow(py - lc / 2, 2) - pow(l1, 2)), l1) + atan2(py - lc / 2, px - la / 2);

    double X = px - la / 2;
    double Y = py - lc / 2;
    double Z = pz - lb / 2;
    double M = sin(th1);
    double N = cos(th1);
    double A = l1 * N + l2 * M;
    double B = -l2 * N + l1 * M;
    double K = pow(l4, 2) * pow(N, 2) - pow((B - Y), 2) - pow(Z, 2) * pow(N, 2) - pow(l3, 2) * pow(N, 2);
    double P = 2 * Z * pow(N, 2) * l3;
    double Q = -2 * (B - Y) * l3 * N;
    //计算th2
    th2_1 = atan2(K, sqrt(pow(P, 2) + pow(Q, 2) - pow(K, 2))) - atan2(Q, P);
    th2_2 = atan2(K, -sqrt(pow(P, 2) + pow(Q, 2) - pow(K, 2))) - atan2(Q, P);
    //计算th3
    th3_1 = asin(-(Z + l3 * sin(th2_1)) / l4) - th2_1;
    th3_2 = asin(-(Z + l3 * sin(th2_2)) / l4) - th2_2;
    switch (leg->configuration)
    {
    case 1:
        leg->servo[0].goal_angle = RAD2ANGLE(th1);
        leg->servo[1].goal_angle = RAD2ANGLE(th2_1);
        leg->servo[2].goal_angle = RAD2ANGLE(th3_1);
        break;
    case 2:
        leg->servo[0].goal_angle = RAD2ANGLE(th1);
        leg->servo[1].goal_angle = RAD2ANGLE(th2_2);
        leg->servo[2].goal_angle = RAD2ANGLE(th3_2);
        break;
    default:
        break;
    }
    // uprintf("foot[%d]:(%f %f %f)\n\r", id, Dog.legs[id].foot_target_x, Dog.legs[id].foot_target_y, Dog.legs[id].foot_target_z);
    // uprintf("inverse reslt %f %f %f\n\r", Dog.legs[id].servo[0].goal_angle, Dog.legs[id].servo[1].goal_angle, Dog.legs[id].servo[2].goal_angle);
}
